#!/usr/bin/python                             
#-*- coding:utf-8 -*- 
import rospy
import os
import sys
import time 
import socket
import json
import ipaddress
import message_filters
from threading import Thread
from robot_pkg.msg import Person1


class Tcp_client(object):

	def __init__(self):
		self.Relays1 = 0 #继电器1
		self.Relays2 = 0 #继电器2
		self.Relays3 = 0 #继电器3
		self.Rocker  = 0 #摇杆方向
		self.Throttle= 50#左右电机油门
		self.Gears   = 0 #吸污电机的档位
		self.StepperDirection = 5 #步进电机方向
		self.pack= {}
		self.data = json.dumps({})
		self.socket_client = socket.socket()
		self.sock_error = 1
		network = "192.168.1.0/24"
		self.clientIP = "192.168.1.10"
		self.port = 8899

		
		# 从txt里读取版本号
		filename = rospy.get_param('file_Version')
		with open(filename,"r+") as f:
			Version   = f.readlines()
			self.pack['Version']   = Version[0].strip()
				
		#建立发布者
		self.person_info_pub = rospy.Publisher('/client_publisher', Person1, queue_size=100)
		#搜索TCP服务端Ip
		while True:
			for ip in ipaddress.IPv4Network(network):
				try:
					sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
					sock.settimeout(0.01)
					result = sock.connect_ex((str(ip),self.port))
					sock.close()
					if result == 0:
						self.clientIP = str(ip)
						print(f"IP{ip}开启里TCP服务器(端口{self.port})")
						return
				except Exception as e:
					print(f"扫描{ip}时出现错误：{e}")

	def connect_to_socket(self):
		while(True):
			if self.sock_error == 1:
				print("等待连接服务器......")
				time.sleep(1)
				try:
					self.socket_client = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
					res=self.socket_client.connect((self.clientIP,self.port))
					time.sleep(0.1)
					if not res:
						self.sock_error = 0
						print("连接服务器成功")
						#break
				except:
					pass
	#错误处理	
	def Dispose_error(self):
		#关闭客户端
		self.sock_error = 1
		self.socket_client.close()
		print("关闭客户端")
		#关闭继电器
		person_msg = Person1()
		person_msg.error = 1 #错误标志
		self.person_info_pub.publish(person_msg)
			
	
	def get_data(self,msg,msg1,msg2,msg3):	
		#新建字典
		pack= {}
		#添加键值
		# pack['angle_x'] 	 = msg.angle_x/32768*180
		# pack['angle_y'] 	 = msg.angle_y/32768*180
		pack['angle_z'] 	 = msg.angle_z/32768*180
		# pack['lon']     	 = msg.longitude
		# pack['lat']     	 = msg.latitude
		pack['Voltage'] 	 = format(msg.Voltage * 0.01,'.2f')	 	#电压
		pack['Current'] 	 = format(msg.Current * 0.01,'.2f')	 	#电流
		pack['Coulomb'] 	 = msg.Coulomb	 				        #剩余容量
		pack['Battery'] 	 = msg.Battery	 	                    #容量百分比
		pack['Temperature1'] = format(msg.Temperature1 * 0.1,'.1f')	#温度1
		pack['camera_ip1']   = msg1.camera_ip1
		pack['camera_ip2']   = msg1.camera_ip2
		pack['Water']        = msg2.Water	
		pack['uwb_x']        = msg3.uwb_x
		pack['uwb_y']        = msg3.uwb_y
		self.data = json.dumps(pack)
		#print(pack)

	def get_camera_ip(self,msg):
		self.pack['camera_ip1']   = msg.camera_ip1
		self.pack['camera_ip2']   = msg.camera_ip2
		
	def get_sensor(self,msg):
		self.pack['Water']        = msg.Water
		
	def get_tcp232(self,msg):
		self.pack['angle_z'] 	 = msg.angle_z/32768*180
		self.pack['Voltage'] 	 = format(msg.Voltage * 0.01,'.2f')	 	#电压
		self.pack['Current'] 	 = format(msg.Current * 0.01,'.2f')	 	#电流
		self.pack['Coulomb'] 	 = msg.Coulomb	 				        #剩余容量
		self.pack['Battery'] 	 = msg.Battery	 	                    #容量百分比
		self.pack['Temperature1'] = format(msg.Temperature1 * 0.1,'.1f')	#温度1
		
	def get_uwb(self,msg):
		self.pack['uwb_x']        = msg.uwb_x
		self.pack['uwb_y']        = msg.uwb_y


	def data_procsssing(self,data):
		#处理数据
		if(int(data[0:2],16)==0x55):#开头
			if(int(data[2:4],16)==0x00):#
				#继电器	
				if(int(data[4:6],16)==0x10):
					if(int(data[6:8]) == 0x01):
						self.Relays1 = int(data[8:10],16)#继电器1
						print("继电器1:",self.Relays1)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
					if(int(data[6:8]) == 0x02):
						self.Relays2 = int(data[8:10],16)#继电器2
						print("继电器2:",self.Relays2)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
					if(int(data[6:8]) == 0x03):
						self.Relays3 = int(data[8:10],16)#继电器3
						print("继电器3:",self.Relays3)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
				#电机档位
				if(int(data[4:6],16)==0x20):
					if(int(data[6:8]) == 0x01):#左右电机档位
						self.Throttle= int(data[8:10],16)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
					if(int(data[6:8]) == 0x02):#吸污电机档位
						self.Gears = int(data[8:10],16)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
				#电机方向
				if(int(data[4:6],16)==0x30):
					if(int(data[6:8]) == 0x01):#左右电机方向
						self.Rocker  = int(data[8:10],16)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
					if(int(data[6:8]) == 0x03):#步进电机方向
						self.StepperDirection  = int(data[8:10],16)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
				#更新程序
				if(int(data[4:6],16)==0x00):
					if(int(data[6:10],16)==0x00ff):
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
						self.Dispose_error()
						print("更新程序")
						os.system('python {}'.format("/home/pi/updata.py"))
					#关机
					if(int(data[6:10],16)==0xffff):
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
						self.Dispose_error()
						print("关机")
						os.system('echo %s | sudo -S %s'%(123456,"shutdown"))
	def client_send(self):
		while(True):
			if self.sock_error == 0:
				try:
					data = json.dumps(self.pack)
					self.socket_client.send(bytes(data.encode("UTF-8")))
					time.sleep(1)
				except socket.error as e:
					print(f"发生错误1: {e}")
					self.sock_error = 1
					self.Dispose_error() 		
					print("重新连接服务器")
					pass
	def client_recv(self):	
		while(True):
			if self.sock_error == 0:
				try:
					#取消超时设置
					self.socket_client.settimeout(None)
					data_from_server = self.socket_client.recv(1024).decode("UTF-8")
					
					person_msg = Person1()
					list_data  = data_from_server.split()#分割字符串
					list_len   = len(list_data)
					for i in range(list_len):
						print("data",list_data[i])
						if len(list_data[i]) == 10:
							self.data_procsssing(list_data[i])
							person_msg.Relays1 = self.Relays1 #继电器1
							person_msg.Relays2 = self.Relays2 #继电器2
							person_msg.Relays3 = self.Relays3 #继电器3
							person_msg.Rocker  = self.Rocker #摇杆方向
							person_msg.Throttle= self.Throttle#左右电机油门
							person_msg.Gears   = self.Gears   #吸污电机档位
							#person_msg.StepperDirection = self.StepperDirection#步进电机方向
							self.person_info_pub.publish(person_msg)
				except socket.error as e:
					print(f"发生错误2: {e}")
					# self.sock_error = 1
					pass
	def main(self):
		# self.connect_to_socket()
		#建立节点
		rospy.init_node('Tcp_client', anonymous=True)
		#建立订阅者
			# tcp232   = message_filters.Subscriber("/tcp232_publisher", Person1)
			# cameraIP = message_filters.Subscriber("/cameraIP_publisher", Person1)
			# sensor   = message_filters.Subscriber("/sensor_publisher", Person1)
			# uwb      = message_filters.Subscriber("/uwb_publisher", Person1)	
			# #集合
			# ts = message_filters.TimeSynchronizer([tcp232,cameraIP,sensor,uwb],10)
			# ts.registerCallback(self.get_data)
		rospy.Subscriber("/tcp232_publisher"	, Person1, self.get_tcp232,queue_size=100)
		rospy.Subscriber("/cameraIP_publisher"	, Person1, self.get_camera_ip,queue_size=100)
		rospy.Subscriber("/sensor_publisher"	, Person1, self.get_sensor,queue_size=100)	
		rospy.Subscriber("/uwb_publisher"		, Person1, self.get_uwb,queue_size=100)	
		
		#建立客户端读和写的线程
		thread  = Thread(target=self.connect_to_socket)
		thread1 = Thread(target=self.client_send)
		thread2 = Thread(target=self.client_recv)
		#线程开始
		thread.start()
		thread1.start()
		thread2.start()	
		#等待线程结束	
		thread.join()
		thread1.join()
		thread2.join()	
		
		rospy.spin()
		


 
 
if __name__ == '__main__':
    try:
        p = Tcp_client()
        p.main()
    except rospy.ROSInterruptException:
        pass
